#include "WarGameLib.h"


wgCamera::wgCamera()
{
	viewDir.z = -1.0f;
	rightVector.x = 1.0f;
}


void wgCamera::setPosition(float x, float y, float z)
{
	position.x = x;
	position.y = y;
	position.z = z;
	upVector.y = 1.0f;
}

vector3_t& wgCamera::getPosition()
{
	return position;
}

void wgCamera::setRotation(float rx, float ry, float rz)
{
	rot.x = rx;
	rot.y = ry;
	rot.z = rz;
}

void wgCamera::UpdateViewMatrix()
{
	
	
	vector3_t viewPoint = position + viewDir;

	gluLookAt(position.x, position.y, position.z,
		viewPoint.x, viewPoint.y, viewPoint.z,
		upVector.x, upVector.y, upVector.z);
	

	UpdateFrustum();


}

void wgCamera::UpdateFrustum()
{
	//static matrix44_t proj;
	static matrix44_t modelview;
	static float t;
	
	//glGetFloatv(GL_PROJECTION_MATRIX, &proj.m[0]);
	glGetFloatv(GL_MODELVIEW_MATRIX, &modelview.m[0]);

	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
		glMultMatrixf(&modelview.m[0]);
		glGetFloatv(GL_PROJECTION_MATRIX, &modelview.m[0]);
	glPopMatrix();
	glMatrixMode(GL_MODELVIEW);

	frustum[0].a = modelview.m[3] - modelview.m[0];
	frustum[0].b = modelview.m[7] - modelview.m[4];
	frustum[0].c = modelview.m[11] - modelview.m[8];
	frustum[0].d = modelview.m[15] - modelview.m[12];

	t = (float) sqrt( SQUARE( frustum[0].a ) + SQUARE( frustum[0].b) + SQUARE(frustum[0].c));


	frustum[0]*= 1.0f / t;


	
	frustum[1].a = modelview.m[3] + modelview.m[0];
	frustum[1].b = modelview.m[7] + modelview.m[4];
	frustum[1].c = modelview.m[11] + modelview.m[8];
	frustum[1].d = modelview.m[15] + modelview.m[12];

	t = (float) sqrt( SQUARE( frustum[1].a ) + SQUARE( frustum[1].b) + SQUARE(frustum[1].c));


	frustum[1]*= 1.0f / t;


	frustum[2].a = modelview.m[3] + modelview.m[1];
	frustum[2].b = modelview.m[7] + modelview.m[5];
	frustum[2].c = modelview.m[11] + modelview.m[9];
	frustum[2].d = modelview.m[15] + modelview.m[13];

	t = (float) sqrt( SQUARE( frustum[2].a ) + SQUARE( frustum[2].b) + SQUARE(frustum[2].c));


	frustum[2]*= 1.0f / t;



	frustum[3].a = modelview.m[3] - modelview.m[1];
	frustum[3].b = modelview.m[7] - modelview.m[5];
	frustum[3].c = modelview.m[11] - modelview.m[9];
	frustum[3].d = modelview.m[15] - modelview.m[13];

	t = (float) sqrt( SQUARE( frustum[3].a ) + SQUARE( frustum[3].b) + SQUARE(frustum[3].c));


	frustum[3]*= 1.0f / t;


	frustum[4].a = modelview.m[3] - modelview.m[2];
	frustum[4].b = modelview.m[7] - modelview.m[6];
	frustum[4].c = modelview.m[11] - modelview.m[10];
	frustum[4].d = modelview.m[15] - modelview.m[14];

	t = (float) sqrt( SQUARE( frustum[4].a ) + SQUARE( frustum[4].b) + SQUARE(frustum[4].c));


	frustum[4]*= 1.0f / t;



	frustum[5].a = modelview.m[3] + modelview.m[2];
	frustum[5].b = modelview.m[7] + modelview.m[6];
	frustum[5].c = modelview.m[11] + modelview.m[10];
	frustum[5].d = modelview.m[15] + modelview.m[14];

	t = (float) sqrt( SQUARE( frustum[5].a ) + SQUARE( frustum[5].b) + SQUARE(frustum[5].c));


	frustum[5]*= 1.0f / t;



}



void wgCamera::MoveForward(float distance)
{
	position = position + viewDir * -distance;
}

void wgCamera::MoveUpward(float distance)
{
	position = position + upVector * distance;
}
void wgCamera::Strafe(float distance)
{
	position = position + rightVector * distance;
}


void wgCamera::RotateX(float angle)
{
	rot.x+=angle;
	viewDir = viewDir * cosf(angle) + upVector * sinf(angle);
	normalize(&viewDir);

	upVector = viewDir * rightVector;
}


void wgCamera::RotateY(float radians)
{
	rot.y+=radians;
	viewDir = viewDir * cosf(radians) + rightVector * sinf(radians);
	normalize(&viewDir);

	rightVector = viewDir * upVector;
}



void wgCamera::RotateZ(float radians)
{
	rot.z+=radians;
	rightVector = rightVector * cosf(radians) + upVector * sinf(radians);
	normalize(&rightVector);

	upVector = viewDir * rightVector;
}



bool wgCamera::SimpleFrustumBoxTest(float x, float y, float z, float width, float height, float depth)
{
	//width*=0.5f;
	//depth*=0.5f;

	 for( int p = 0; p < 6; p++ )
	 {
		 if( frustum[p].a * (x - width) + frustum[p].b * (y - height) + frustum[p].c * (z - depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x + width) + frustum[p].b * (y - height) + frustum[p].c * (z - depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x - width) + frustum[p].b * (y + height) + frustum[p].c * (z - depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x + width) + frustum[p].b * (y + height) + frustum[p].c * (z - depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x - width) + frustum[p].b * (y - height) + frustum[p].c * (z + depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x + width) + frustum[p].b * (y - height) + frustum[p].c * (z + depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x - width) + frustum[p].b * (y + height) + frustum[p].c * (z + depth) + frustum[p].d > 0 )
			 continue;
		 if( frustum[p].a * (x + width) + frustum[p].b * (y + height) + frustum[p].c * (z + depth) + frustum[p].d > 0 )
			 continue;
		 return false;
	 }
   return true;
}

